#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

void test01() {
    Eigen::Vector3f ppp;
    Eigen::Vector3f ppp2(1, 1, 1);
    // Eigen::Vector3f p2(0, 1, 0);
    Eigen::Vector3f p1(1, 1, 0);
    Eigen::Vector3f p2(0, 10, 0);

    std::cout << "---------------------------------------dot" << std::endl;
    float p_dot1 = p1.dot(p2) / (p1.norm() * p2.norm());
    std::cout << p_dot1 << std::endl;

    // p1.normalize();
    // std::cout << p1 << std::endl;

    std::cout << "---------------------------------------cross1" << std::endl;
    Eigen::Vector3f p_c1 = p1.cross(p2);
    std::cout << p_c1 << std::endl;
    std::cout << p_c1.norm() << std::endl;

    p1.normalize();
    p2.normalize();
    std::cout << "---------------------------------------dot" << std::endl;
    float p_dot2 = p1.dot(p2) / (p1.norm() * p2.norm());
    std::cout << p_dot2 << std::endl;
    std::cout << "---------------------------------------cross2" << std::endl;
    Eigen::Vector3f pc2 = p1.cross(p2);
    std::cout << pc2 << std::endl;
    std::cout << pc2.norm() << std::endl;
}

void avoid() {
    Eigen::Vector3f vel(0, 0, 0);
    Eigen::Vector3f n1(1, 0, 0);
    Eigen::Vector3f n2(0, 1, 0);

    Eigen::Vector3f p1(0, -1, 0);
    Eigen::Vector3f p2(0, 1, 0);

    Eigen::Vector3f _vel(0, 0, 0), r(0, 0, 0);
    r = p1 - p2;

    float theta1 = r.dot(n1);
    float theta2 = r.dot(n2);

    if (abs(theta1) < abs(theta2)) {
        _vel = r.normalized().cross(n1);
    } else {
        _vel = r.normalized().cross(n2);
    }
    // 避碰速度系数，距离越近，避碰速度应该越大，距离越远，避碰速度应该越小
    float ratio = 40.0 / r.norm();
    _vel *= ratio;

    std::cout << "=============================avoid\n";
    std::cout << "|r|=" << r.norm() << ", ratio=" << ratio << std::endl;
    std::cout << "p1=" << p1.transpose() << std::endl;
    std::cout << "p2=" << p2.transpose() << std::endl;
    std::cout << "_vel=" << _vel.transpose() << ", |_vel|=" << _vel.norm() << std::endl;
}
int main(int argc, char *argv[]) {
    // test01();
    avoid();
    return 0;
}
